#include "FrcDriveSubsystem.h"
#include "FrcConstants.h"

FrcDriveSubsystem::FrcDriveSubsystem(){
	
	m_leftJoystick = Joystick::GetStickForPort(c_joystickLeftPort);
	m_rightJoystick = Joystick::GetStickForPort(c_joystickRightPort);
	m_leftFrontDriveMotor = new Talon(c_leftFrontDriveMotor);
	m_leftRearDriveMotor = new Talon(c_leftRearDriveMotor);
	m_rightFrontDriveMotor = new Talon(c_rightFrontDriveMotor);
	m_rightRearDriveMotor = new Talon(c_rightRearDriveMotor);
	m_driveTimer = new Timer();
	// Robot Drive Function documentation is wrong. left rear, left front, right front, right rear is correct order.
	
}
FrcDriveSubsystem::~FrcDriveSubsystem()
{
	
}

void FrcDriveSubsystem::DoRobotInit(void)
{
	
}

void FrcDriveSubsystem::DoTeleopInit(void)
{
	
}

void FrcDriveSubsystem::DoDisabledInit(void)
{

}

void FrcDriveSubsystem::DoTeleopPeriodic(void)
{
	FrcMecanumDrive(m_leftJoystick,m_rightJoystick);
}

void FrcDriveSubsystem::DoDisabledPeriodic(void)
{
	
}


void FrcDriveSubsystem::DoAutonomousPeriodic(void)
{
	
}

void FrcDriveSubsystem::DoAutonomousInit(void)
{
	m_driveTimer->Stop();
	m_driveTimer->Reset();
	m_driveTimer->Start();
	
}

void FrcDriveSubsystem::FrcMecanumDrive(Joystick *js_left, Joystick *js_right)
{
	double leftFront, rightFront, leftRear, rightRear;
	double jx, jy, jr; //jx is x translation, jy is y translation, jr is rotation
	double rjx,rjy,ljx;

	rjx = m_rightJoystick->GetX();
	rjy = m_rightJoystick->GetY();
	ljx = m_leftJoystick->GetX();
			
// Deadzone 
		if( rjx > -.1 && rjx < .1)
				{
				
					jx = 0;
			
				}
				else
				{
					jx = rjx;
				}
				if (ljx > -.1 && ljx < .1)
				{
					jy = 0;
				}
				else
				{
					 jy = ljx * -1;
				}
				if(rjy > -.1 && rjy < .1)
				{
					jr = 0;
				}
				else
				{
					jr = rjy;
				}
// Mecanum Drive Logic				
	leftFront =  jx + jy + jr;
	rightFront = - jx + jy - jr;
	leftRear = - jx +jy + jr;
	rightRear = jx + jy - jr;
// Apply Logic to Motors
	m_leftFrontDriveMotor->Set(leftFront);
	m_rightFrontDriveMotor->Set(rightFront);
	m_leftRearDriveMotor->Set(leftRear);
	m_rightRearDriveMotor->Set(rightRear);

	
	
}

double FrcDriveSubsystem::GetDriveTime()
{
	return m_driveTimer->Get();
	
}

void FrcDriveSubsystem::SetTargetTimeInSeconds(double seconds)
{
	m_driveTimeTarget = seconds;
}

bool FrcDriveSubsystem::IsDriveByTimeFinished()
{
	if(m_driveTimer->Get() >= m_driveTimeTarget)
	{
		return true;
	} 
	else 
	{
		return false;
	}
}
void FrcDriveSubsystem::ResetTimer()
{
	m_driveTimer->Stop();
	m_driveTimer->Reset();
	m_driveTimer->Start();
}
void FrcDriveSubsystem::DriveByTime()
{

	
	if (m_driveTimer->Get() < m_driveTimeTarget)
	{
		m_leftFrontDriveMotor->Set(-.75);
		m_rightFrontDriveMotor->Set(.75);
		m_leftRearDriveMotor->Set(-.75);
		m_rightRearDriveMotor->Set(.75);
	} 
	else if(m_driveTimer->Get() > m_driveTimeTarget)
	{
		m_leftFrontDriveMotor->Set(0);
		m_rightFrontDriveMotor->Set(0);
		m_leftRearDriveMotor->Set(0);
		m_rightRearDriveMotor->Set(0);
		m_driveTimer->Stop();
	}

	
	
}

void FrcDriveSubsystem::DriveByTimeReverse()
{
	
	
	if (m_driveTimer->Get() < m_driveTimeTarget)
	{
		m_leftFrontDriveMotor->Set(.75);
		m_rightFrontDriveMotor->Set(-.75);
		m_leftRearDriveMotor->Set(.75);
		m_rightRearDriveMotor->Set(-.75);
	} 
	else if(m_driveTimer->Get() > m_driveTimeTarget)
	{
		m_leftFrontDriveMotor->Set(0);
		m_rightFrontDriveMotor->Set(0);
		m_leftRearDriveMotor->Set(0);
		m_rightRearDriveMotor->Set(0);
		m_driveTimer->Stop();
	}
} 


